#include "A-Star.h"
#include "Level.h"
#include <algorithm>

AStar::AStar()
:inter(0)
{
	m_Start = NULL;
	m_End = NULL;
}
void AStar::clearnodes()
{
	vector<ANode*>::iterator iter = m_Nodes.begin();
	vector<ANode*>::iterator iterEnd = m_Nodes.end();

	for ( ; iter != iterEnd; ++iter)
	{
		(*iter)->m_State = 0;
	}
}

ANode* AStar::GetPath(ANode *s_Start, ANode* s_End)
{
	if (s_Start == NULL)
		return NULL;
	if (s_End == NULL)
		return NULL;
	if (s_Start == s_End)
		return NULL;

	vector<ANode*>::iterator iter = s_Start->m_NodeList.begin();
	vector<ANode*>::iterator iterEnd = s_Start->m_NodeList.end();

	ANode* temp = NULL;
	ANode* prev = NULL;
	float Distance = 0.0f;
	int size = s_Start->m_NodeList.size();
	int count = 0;

	//Check if Any of the Adjacent Nodes are the End;
	for ( ; iter != iterEnd ; ++iter)
	{
		if((*iter) == s_End)
		{
 			s_Start->m_Next = s_End;
			s_End->m_Parent = s_Start;
			return NULL;
		}
	}

	iter = s_Start->m_NodeList.begin();
	iterEnd = s_Start->m_NodeList.end();

	//Go Through the list of Nodes s_Start is connected to
	//Fill in with Data;
	for ( ; iter != iterEnd ; ++iter)
	{
		if(((*iter)->m_State == 0))
		{
			if (temp == NULL)
			{
				temp = (*iter);
				//set as temp node
				Distance = abs(( (temp->m_Pos.x - s_End->m_Pos.x) + (temp ->m_Pos.y - s_End->m_Pos.y) ));
				//Get Distance to s_End
				temp->m_H = Distance;
				//set as that Nodes m_H
				temp->m_G = abs(( (s_Start->m_Pos.x - temp->m_Pos.x) + (s_Start ->m_Pos.y - temp->m_Pos.y) )) + s_Start->m_G;
				//set m_G as the distance between the Parent Node and this node
				temp->m_F = temp->m_G + temp->m_H;
				//set m_F as m_G + m_H
				prev = temp;
				(*iter)->m_State = 1;

			} else 
			{
				//Get Distance to the End Node
				Distance = abs(( ((*iter)->m_Pos.x - s_End->m_Pos.x) + ((*iter) ->m_Pos.y - s_End->m_Pos.y) ));
				temp = (*iter);
				//set as temp node
				temp->m_H = Distance;
				//set as that Nodes m_H
				temp->m_G = abs(( (temp->m_Pos.x - prev->m_Pos.x) + (temp->m_Pos.y - prev->m_Pos.y) )) + s_Start->m_G;
				//set m_G as the distance between the Parent Node and this node
				temp->m_F = temp->m_G + temp->m_H;
				//set m_F as m_G + m_H
				prev = temp;
				(*iter)->m_State = 1;
			}
		}	
	}

	temp = NULL;

	//look thru m_Nodes for any node that is stateOpen
	//Find the Lowest Cost
	//Get Path from that node
	
	iter = m_Nodes.begin();
	iterEnd = m_Nodes.end();
	for ( ; iter != iterEnd ; ++iter)
	{
		if(((*iter)->m_State == 1))		//Open
		{
			++count;
			if (temp == NULL)
			{
				temp = (*iter);
				prev = temp;

			} else 
			{
				//if this node is cheaper then the previous Node
				if ((*iter)->m_F < prev->m_F)
				{
					temp = (*iter);
					prev = temp;
				}
			}
		}

	}
	
	if(temp == NULL)
		return NULL;

	temp->m_Parent = s_Start;
	s_Start->m_Next = temp;
	
	/*
	*	DEBUG
	*/
	//++inter;
	//if(inter == 5)
	//{
	//	inter = 0;
	//	return NULL;
	//}

	s_Start->m_State = 2;	//Close This Node

	GetPath(temp,s_End);

	return temp;
}

void AStar::CreateNode(const Vector2 &s_Pos)
{
	m_Nodes.push_back(new ANode(s_Pos));
}
void AStar::ConnectNodes(ANode* s_One, ANode* s_Two)
{

	//Check to see if s_Two is already Attached
	if(find(s_One->m_NodeList.begin(),s_One->m_NodeList.end(),s_Two) == s_One->m_NodeList.end())
	{
		s_One->m_NodeList.push_back(s_Two);
	}

	if(find(s_Two->m_NodeList.begin(),s_Two->m_NodeList.end(),s_One) == s_Two->m_NodeList.end())
	{
		s_Two->m_NodeList.push_back(s_One);
	}
	
}

int AStar::NodeCount() const
{return m_Nodes.size();}

ANode* AStar::GetNode(int s_Index)
{return m_Nodes[s_Index];}
